Skip to content

fix: wait for small moves to settle before reporting arrival - #49

Open
yd-sl wants to merge 1 commit into
mainfrom
fix/small-move-arrival
Open

yd-sl wants to merge 1 commit into
mainfrom
fix/small-move-arrival

Conversation

@yd-sl

@yd-sl yd-sl commented Nov 7, 2026

Copy link
Copy Markdown
Contributor

Summary

Arm._arrive could report a small joint-space move as arrived before the arm left its start pose. The nominal q_tol=0.03 rad is larger than the whole displacement of a 3 mm move (about 0.008 rad), so the first still frames at the start already satisfied the arrival criterion. On hardware (7J LiteArm, firmware Litearm1.11.1-7J) a 3 mm move_pose returned at 0.154 s while the motion started at 0.18 s and settled at 0.94 s; a caller could send the next command to an arm that was still moving.

Changes

  • _arrive now accepts the pre-command q as start. Displacements between 5 mrad and 0.12 rad are treated as small moves: the tolerance is tightened to max(5 mrad, delta/4), at least half of the displacement must be covered, and 200 ms of continuous stillness is required (covering the firmware hold handoff). Large moves, and calls without start, keep the previous behavior exactly.
  • movej, movej_sync and home capture the pre-command state and pass it through.
  • Add tests/test_arrive_small_move.py: a paced FakeTransport pushes 10 status frames at the start pose and then the target frames. It fails on the old code (returns the start q) and passes with the change.

Testing

python3 -m unittest discover -s tests -v
Ran 5 tests in 0.315s
OK
  • The new regression test fails on pristine main (AssertionError: 0.0 != 0.008 within 6 places) and passes with the patch.
  • Real-arm retest (7J, Litearm1.11.1-7J, speed 0.1), guarded 2/5/10 mm moves:
    • 10 mm: 0.70 s return, 0.74 mm error, FK vs firmware 7.1e-08 m. Before the patch it returned at 0.20 s and the same 0.2 s-later check measured 4.97 mm.
    • 5 mm: 0.50 mm error, 0.54 s return, FK vs firmware 5.2e-08 m.
    • 2 mm: below the 5 mrad lower bound, so it keeps the old path; measured error 0.81 mm.
  • The same patch was also applied on top of PR feat: add SDK-local kinematics and guarded pose PTP #48's head (5ce0cbf) and passed the full suite (42 passed, 1 skipped) plus the real-arm checks.

Issues

Related to #48 (found during its real-arm review). No separate issue was opened.

Small joint-space moves were reported as arrived before the arm left its
start pose: q_tol=0.03 rad is larger than the whole displacement (a 3 mm
move is about 0.008 rad), so the first still frames at the start satisfied
the criterion. On hardware move_pose returned at 0.154 s while motion
started at 0.18 s and settled at 0.94 s.

When the commanded displacement is between 5 mrad and 0.12 rad, _arrive
now tightens the tolerance to max(5 mrad, delta/4), requires half of the
displacement to be covered, and requires 200 ms of continuous stillness to
cover the firmware hold handoff. movej, movej_sync and home pass the
pre-command q as start; large moves keep the previous behavior exactly.

Real-arm retest (Litearm1.11.1-7J): a 10 mm step now returns after 0.70 s
with 0.74 mm error, instead of returning early and measuring 4.97 mm.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant